import serial
import time
import re


class ChessRobot:
    """围棋机器人控制类"""

    def __init__(self, config=None):
        """初始化机器人，可传入自定义配置"""
        # 默认配置参数
        self.config = config or {
            'serial_port1': '/dev/ttyCH341USB0',  # 主串口(运动控制)
            'serial_port2': '/dev/ttyS9',  # 辅助串口(吸放控制)
            'baudrate': 115200,  # 波特率
            'delay': 0.4,  # 命令间延迟时间(秒)
            'board_origin_x': 0.0,  # 棋盘原点X偏移量(mm)
            'board_origin_y': 0.0,  # 棋盘原点Y偏移量(mm)
            'grid_size': 15.0,  # 棋盘格子大小(mm)
            'board_size': 13,  # 棋盘路数
            'white_piece_x': 15.0,  # 白棋位置X坐标
            'white_piece_y': 15.0,  # 白棋位置Y坐标
            'black_piece_x': 15.0,  # 黑棋位置X坐标
            'black_piece_y': 15.0,  # 黑棋位置Y坐标
            'standby_x': 0.0,  # 待机位置X坐标
            'standby_y': 180.0,  # 待机位置Y坐标
            'move_speed': 10000,  # 移动速度
            'lift_height': -20.0,  # 抬笔高度
            'drop_height': -3.0  # 落笔高度
        }

        # 初始化两个串口
        self.ser1 = serial.Serial(self.config['serial_port1'], self.config['baudrate'], timeout=1)
        self._ser2 = serial.Serial(self.config['serial_port2'], self.config['baudrate'], timeout=1)
        time.sleep(2)  # 等待串口初始化
        self.initialize_robot()

    @property
    def ser2(self):
        """获取串口2的引用，以便在其他函数中使用"""
        return self._ser2

    def initialize_robot(self):
        """初始化机器人，设置原点、抬笔并移动到待机位置"""
        self.send_gcode("G10 P0 L20 X0.0 Y0.0 Z0")  # 设置当前位置为原点
        self.lift_piece()
        self.move_to(self.config['standby_x'], self.config['standby_y'])  # 移动到待机位置

    def send_gcode(self, cmd):
        """通过串口1发送G代码命令"""
        print(f"GCODE: {cmd}")
        self.ser1.write((cmd + "\n").encode())
        time.sleep(self.config['delay'])

    def send_gripper(self, cmd):
        """通过串口2发送吸盘控制命令"""
        if cmd.lower() == "open":
            # 十六进制格式: 65 01 02 00 FF FF FF
            hex_cmd = bytes([0x65, 0x01, 0x02, 0x00, 0xFF, 0xFF, 0xFF])
            print("GRIPPER: open (hex: 65 01 02 00 FF FF FF)")
        elif cmd.lower() == "close":
            # 十六进制格式: 65 01 03 00 FF FF FF
            hex_cmd = bytes([0x65, 0x01, 0x03, 0x00, 0xFF, 0xFF, 0xFF])
            print("GRIPPER: close (hex: 65 01 03 00 FF FF FF)")
        else:
            raise ValueError("Invalid gripper command, must be 'open' or 'close'")

        self._ser2.write(hex_cmd)
        time.sleep(self.config['delay'])

    def lift_piece(self):
        """抬笔"""
        self.send_gcode(f"G1 G90 Z{self.config['lift_height']} F{2500}")

    def drop_piece(self):
        """落笔"""
        self.send_gcode(f"G1 G90 Z{self.config['drop_height']} F{2500}")

    def move_to(self, x, y):
        """移动到指定坐标"""
        self.send_gcode(f"G1 X{x} Y{y} F{self.config['move_speed']}")

    def position_to_coords(self, position):
        """将棋位(A1,N13等)转换为坐标"""
        # 使用正则表达式匹配输入格式
        match = re.match(r'^([A-N])(1[0-3]|[1-9])$', position.upper())
        if not match:
            raise ValueError(f"Invalid position format: {position}")

        col_char = match.group(1)
        row_str = match.group(2)

        row = int(row_str)
        col_idx = ord(col_char) - ord('A')  # A=0, B=1, ..., N=13
        row_idx = row - 1  # 1=0, 2=1, ..., 13=12

        if col_idx < 0 or col_idx >= self.config['board_size']:
            raise ValueError(f"Column must be between A and {chr(ord('A') + self.config['board_size'] - 1)}")
        if row_idx < 0 or row_idx >= self.config['board_size']:
            raise ValueError(f"Row must be between 1 and {self.config['board_size']}")

        # 计算坐标
        x = self.config['board_origin_x'] + col_idx * self.config['grid_size']
        y = self.config['board_origin_y'] + row_idx * self.config['grid_size']

        return x, y

    def pick_piece(self, color):
        """拾取棋子"""
        if color.upper() == 'W':
            piece_x, piece_y = self.config['white_piece_x'], self.config['white_piece_y']
        elif color.upper() == 'B':
            piece_x, piece_y = self.config['black_piece_x'], self.config['black_piece_y']
        else:
            raise ValueError("Color must be 'W' or 'B'")

        # 移动到棋子位置
        self.move_to(piece_x, piece_y)

        # 落笔
        self.drop_piece()
        time.sleep(self.config['delay'])

        # 打开吸盘拾取棋子
        self.send_gripper("open")
        time.sleep(self.config['delay'] * 2)  # 拾取需要更长时间

        # 抬笔
        self.lift_piece()

    def place_piece(self, x, y):
        """放置棋子到指定位置"""
        # 移动到目标位置
        self.move_to(x, y)

        # 落笔
        self.drop_piece()
        time.sleep(self.config['delay'])

        # 关闭吸盘释放棋子
        self.send_gripper("close")
        time.sleep(self.config['delay'] * 2)  # 释放需要更长时间

        # 抬笔
        self.lift_piece()

    def make_move(self, command):
        """执行完整的下棋动作"""
        try:
            # 解析命令 (W A1 或 B K10)
            parts = command.split()
            if len(parts) != 2:
                raise ValueError("Command format: [W/B] [A1-N13]")

            color = parts[0].upper()
            position = parts[1].upper()

            # 获取目标坐标
            target_x, target_y = self.position_to_coords(position)
            print(f"Placing {color} piece at {position} ({target_x}, {target_y})")

            # 1. 拾取棋子
            self.pick_piece(color)

            # 2. 放置棋子到棋盘
            self.place_piece(target_x, target_y)

            # 3. 返回待机位置
            self.move_to(self.config['standby_x'], self.config['standby_y'])

        except ValueError as e:
            print(f"Error: {e}")

    def close(self):
        """关闭串口连接"""
        self.ser1.close()
        self._ser2.close()